/*
 * robot.cpp
 *
 *  Created on: Sep 30, 2014
 *      Author: amorim
 */
#include <cv.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "util.h"
#include "robot.h"

using namespace std;

Robot::Robot() {
	device = "/dev/rfcomm0";
	init_cmd();
}

int Robot::connect() {
	fd = connection.openSerialPort(device.c_str());
	return fd;
}

void Robot::init_cmd() {
	commands[MOVE_FWD] = "FW";
	commands[MOVE_BWD] = "BW";
	commands[LEFT_FWD] = "FW";
	commands[LEFT_FWD] = "LF";
	commands[RIGHT_FWD] = "RF";
	commands[LEFT_BWD] = "LB";
	commands[RIGHT_BWD] = "RB";
	commands[STOP] = "ST";
	commands[HIGH_SPEED] = "HS";
	commands[LOW_SPEED] = "LS";
	commands[BEEP] = "BP";
}

void Robot::move(ROBOT_CMD cmd) {
	//printf(" CMD: %s ", commands[cmd].c_str());
	connection.tx_message(fd, 3, commands[cmd].c_str());
}

void Robot::close() {
	connection.close(fd);
}

